-
Notifications
You must be signed in to change notification settings - Fork 451
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Feature/filtered elevation layer #84
base: master
Are you sure you want to change the base?
Conversation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Please have a look at our style guide.
Following the variable / function naming rules of costmap2d might make sense here, however make sure to use proper indents and formatting rules (s.a braces in short ifs). Also use doxygen style comments more consistently, look at the headers in elevation_mapping they are nicely documented.
elevation_layer/CMakeLists.txt
Outdated
@@ -0,0 +1,77 @@ | |||
# use c++11 | |||
set(CMAKE_CXX_COMPILER_ARG1 -std=c++11) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Put this in the build section of the cmake and use
add_definitions(-std=c++11)
elevation_layer/CMakeLists.txt
Outdated
) | ||
|
||
## cpp-header files for installation | ||
install(DIRECTORY include/ |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think this should be:
install(DIRECTORY include/${PROJECT_NAME}/
elevation_layer/CMakeLists.txt
Outdated
cmake_minimum_required(VERSION 2.8.3) | ||
project(elevation_layer) | ||
|
||
find_package(Boost REQUIRED COMPONENTS thread) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You never actually use boost in the cmake.
See here for a how-to.
elevation_layer/CMakeLists.txt
Outdated
) | ||
|
||
## declare a cpp library | ||
add_library(elevation_layer |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
FYI you can use ${PROJECT_NAME} whenever you have elevation_layer.
@@ -0,0 +1,82 @@ | |||
// |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Follow this packages file naming:
e.g.
/*
* ElevationMapping.cpp
*
* Created on: Nov 12, 2013
* Author: Péter Fankhauser
* Institute: ETH Zurich, ANYbotics
*/
} | ||
bool track_unknown_space; | ||
nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown()); | ||
if (track_unknown_space) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Use curly braces(styleguide)! Or shorthand default_value = track_unknown_space ? NO_INFORMATION : FREE_SPACE;
std::lock_guard<std::mutex> lock(elevation_map_mutex_); | ||
if (rolling_window_) | ||
updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); | ||
if (!(enabled_ && elevation_map_received_)) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Makes more sense to do this before updating origin. Right?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think you still want your local costmap to move around with the robot even though the elevation_mapping is not received. But actually is kind of the same
if (!footprint_clearing_enabled_) return; | ||
costmap_2d::transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); | ||
|
||
for (unsigned int i = 0; i < transformed_footprint_.size(); i++) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Use range based for:
for(auto&& point : transformed_footprint_) {
touch(point.x, point.y, min_x, min_y, max_x, max_j);
}
} | ||
if ( elevation_data(gridmap_index(0), gridmap_index(1)) > height_treshold_ ) // If point too high, it could be an obstacle | ||
{ | ||
if (!has_edges_layer) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You could check this before even going into the for loop, since this is a property of the elevation_map. Also use a throttled message.
|
||
switch (combination_method_) | ||
{ | ||
case 0: // Overwrite |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Use a string (that ideally maps to an enum) for combination_method_.
elevation_layer/CMakeLists.txt
Outdated
include_directories(include) | ||
include_directories( | ||
${catkin_INCLUDE_DIRS} | ||
) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
you can have a single include_directories and list the elements
// | ||
|
||
#ifndef ELEVATION_LAYER_H | ||
#define ELEVATION_LAYER_H |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You can use #pragma once
, a more modern way of doing this.
elevation_layer/package.xml
Outdated
@@ -0,0 +1,32 @@ | |||
<?xml version="1.0"?> | |||
<package> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I would use package version 2 here, so you only have <depend>
instead of <build_depend>
and <run_depend>
elevation_layer/package.xml
Outdated
<run_depend>costmap_2d</run_depend> | ||
<run_depend>dynamic_reconfigure</run_depend> | ||
<run_depend>grid_map_ros</run_depend> | ||
<run_depend>filters</run_depend> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
const std::string tf_prefix = tf::getPrefixParam(prefix_nh); | ||
|
||
// get parameters from config file | ||
if(!nh.param("elevation_topic", elevation_topic_, std::string(""))) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If you use param_io
(in repo any_node
), it will print warnings for you and you can shorten the code here.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I would not add this dependency in a package that could be used completely independently of our software stack.
elevation_layer/package.xml
Outdated
<!-- <author email="echisari@anybotics.com">Eugenio Chisari</author> --> | ||
<maintainer email="echisari@anybotics.com">Eugenio Chisari</maintainer> | ||
|
||
<license>BSD</license> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Are we open sourcing this plugin?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I dont know
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
It's already open source, if we keep it in this repo 😉
protected: | ||
std::string global_frame_; ///< @brief The global frame for the costmap | ||
std::vector<boost::shared_ptr<message_filters::SubscriberBase> > elevation_subscribers_; ///< @brief Used for the observation message filters | ||
dynamic_reconfigure::Server<elevation_layer::ElevationPluginConfig> *dsrv_; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If you use a smart pointer here, you do not need to care about memory handling (calling delete
)
|
||
using costmap_2d::NO_INFORMATION; | ||
using costmap_2d::LETHAL_OBSTACLE; | ||
using costmap_2d::FREE_SPACE; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think that we soon need more than binary traversability information. Imagine you have two paths to reach the goal, one path on flat terrain and one path with ramps/steps etc. Both paths are traversable, but one has higher costs. In these cases the planner will need get these costs through the map. Therefore we will need traversability information between 0 and 1.
Unrelated to that, during testing we saw that noise could result to untraversable cells in the costmap. In these cases the planner failed to find a path. In my opinion it would make sense to have a threshold below which untraversable cells are not considered to be an obstacle.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
About the first point, yes its true, this one is the short term solution, in the future we will need a non-binary cost. About the second one, it is already implemented like this :)
if ( elevation_data(gridmap_index(0), gridmap_index(1)) > height_treshold_ ) // If point too high, it could be an obstacle | ||
{ | ||
if (!has_edges_layer) { | ||
ROS_WARN("No edges layer found !!"); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If this check fails, the warning will be printed many times. Can you do the check outside the for loop?
virtual void setupDynamicReconfigure(ros::NodeHandle& nh); | ||
|
||
|
||
int combination_method_; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You could use an enum
or an enum class
for this.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I don't think it is possible to get an enum type from a yaml file, is it?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I don't think so, but you could use a string that you transform to an enum. This makes it easier for the user to tweak the parameters of your layer (0, 1 or 2 is not very descriptive)
rolling_window_ = layered_costmap_->isRolling(); | ||
|
||
ElevationLayer::matchSize(); | ||
current_ = true; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Value of current_
is used by move_base
to know if the layer is up-to-date (look here). Therefore it should be updated depending on elevation layer updates. For example if we have not received elevation map for certain period we should make this value to false so that move_base
gives a warning and stops the robot.
filters_configuration_loaded_ = false; | ||
global_frame_ = layered_costmap_->getGlobalFrameID(); | ||
elevation_layer_name_ = "elevation"; | ||
edges_layer_name_ = "edges"; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Since both elevation_layer_name_
and edges_layer_name_
come from elevation mapping, shouldn't the names be configurable using parameters?
current_ = true; | ||
elevation_map_received_ = false; | ||
filters_configuration_loaded_ = false; | ||
global_frame_ = layered_costmap_->getGlobalFrameID(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
global_frame_
is never used in the code!
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
AFAIK, the elevation layer can only work if the costmap frame is set to odom
(or same as the frame used by elevation mapping). The user should be warned if the frames in costmap and elevation layers are not matching.
{ | ||
std::lock_guard<std::mutex> lock(elevation_map_mutex_); | ||
elevation_map_ = filtered_map; | ||
height_treshold_ /= 2; // Half the treshold since the highest sharpness is at midheigth of the obstacles |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
threshold
(instead of treshold
) is official spelling in English :)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks, but this PR is not close to be merged. No documentation, wrong formatting, and some open issues from other reviewers. Please work on this before I take another look.
elevation_layer/README.md
Outdated
@@ -0,0 +1,3 @@ | |||
# Elevation layer |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Don't add README files for each package, but please document the new software in the main README.
input_layer: elevation | ||
output_layer: edges | ||
expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation)' # Edge detection. | ||
# expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation)' # Sharpen. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Delete unused expressions.
@@ -0,0 +1,12 @@ | |||
elevation_filters: | |||
# Edge detection on elevation layer with convolution filter as alternative to filter above. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This comment makes no sense because there's no filter above?
expression: 'sumOfFinites([0,1,0;1,-4,1;0,1,0].*elevation)' # Edge detection. | ||
# expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation)' # Sharpen. | ||
compute_empty_cells: false | ||
edge_handling: mean # options: inside, crop, empty, mean |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The comments here are instructions for the grid map filter example, not fitting here.
|
||
#pragma once | ||
|
||
#include <atomic> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Please group and order the includes correctly, see coding guidelines.
#include <filters/filter_chain.h> | ||
#include <message_filters/subscriber.h> | ||
#include <ros/ros.h> | ||
#include "grid_map_ros/GridMapRosConverter.hpp" |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Use correct < and "
*/ | ||
void onInitialize() override; | ||
|
||
/** |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Everywhere: Use correct doxygen formatting!
} | ||
|
||
void ElevationLayer::onInitialize() { | ||
nh_ = ros::NodeHandle("~/" + name_); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
No abbreviations! See programming guidelines.
|
||
ElevationLayer::matchSize(); | ||
current_ = true; | ||
elevation_map_received_ = false; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Everywhere: Use correct formatting for variables.
I don't think |
The other names are inflation_layer, obstacle_layer, static_layer, voxel_layer. |
…red_elevation_layer
@pfankhauser Hi, addressed the comments you wrote, i hope i didn't miss anything. |
|
||
This is a plugin for [costmap_2d] and provides an implementation of a 2d costmap. With the `elevation_mapping_costmap_2d_plugin` package, the generated elevation map can be used to build a costmap_2d to work with the [ROS] [navigation] stack. It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d. | ||
|
||
The layer applies a height and a sharpness filter to the incoming data. The behaviour can be adjusted tuning the parameters `heightThreshold_` and `edgesSharpnessThreshold_`. The former labels as obstacles all data above a certain height, while the latter labels as obstacles data above a certain edge sharpness, where a sharp edge is defined as a sudden change in height; this is used to avoid labeling slopes as obstacle. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
heightThreshold_
is this an absolute height or relative to the neighbor cells?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Absolute
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
So is this really sensible when the robot's position drifts over time?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We did't experience that. I mean, even if the robot drifts in map frame, the sensor still outputs proper depth values and elevation mapping is built on those, irrespective of the drift (right? I am not that sure anymore, i should doublecheck)
@@ -235,6 +235,11 @@ This is the main Robot-Centric Elevation Mapping node. It uses the distance sens | |||
|
|||
The data for the sensor noise model. | |||
|
|||
### elevation_mapping_costmap_2d_plugin | |||
|
|||
This is a plugin for [costmap_2d] and provides an implementation of a 2d costmap. With the `elevation_mapping_costmap_2d_plugin` package, the generated elevation map can be used to build a costmap_2d to work with the [ROS] [navigation] stack. It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Whats the message type of costmap_2d, OccupancyGrid? It would be good to state this.
globalFrame_ = layered_costmap_->getGlobalFrameID(); | ||
|
||
// get parameters from config file | ||
if (!nodeHandle_.param("elevation_topic", elevationTopic_, std::string(""))) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
All these parameters are not described in the README?
enum CombinationMethod { Overwrite, Maximum, Unknown }; | ||
|
||
/*! | ||
* converts the string from the yaml file to the proper CombinationMethod enum |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
All comments should start with a capital letter and end with a period, see coding guidelines.
@harmishhk @hogabrie @pfankhauser Since we will not use costmap_2d, this is not needed anymore, right? Or Peter, do you want it merged anyway? Otherwise it can be closed. |
We will not use costmap_2d in the future, but we are using at the moment, and I also think this could be useful for community if we merge. Moreover, the functionality of filtering at specific height will be needed also with grid_map based cost-map that we are going to use in the future. |
} | ||
|
||
void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nodeHandle_) { | ||
dsrv_.reset(new dynamic_reconfigure::Server<elevation_mapping_costmap_2d_plugin::ElevationPluginConfig>(nodeHandle_)); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Please refrain from using abbreviations.
|
||
void ElevationLayer::setupDynamicReconfigure(ros::NodeHandle& nodeHandle_) { | ||
dsrv_.reset(new dynamic_reconfigure::Server<elevation_mapping_costmap_2d_plugin::ElevationPluginConfig>(nodeHandle_)); | ||
dynamic_reconfigure::Server<elevation_mapping_costmap_2d_plugin::ElevationPluginConfig>::CallbackType cb = |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Same here, please refrain from using abbreviations.
void ElevationLayer::reset() { | ||
deactivate(); | ||
resetMaps(); | ||
current_ = true; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Current what? Please use clearer variable names.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
All those variables are derived from base classes in costmap_2d .....
@harmishhk and @pfankhauser what should we do about this PR? |
@chisarie is not working on this any more. There are some changes needed for this PR before we can merge it, as long as nobody needs it, I suggest to leave it in this branch for later reference. |
A plugin for [costmap_2d]](http://wiki.ros.org/costmap_2d). It takes the elevation map as input and generates a layer of obstacles in the local costmap_2d.